import numpy
import math

#https://ccrma.stanford.edu/courses/120-fall-2003/lecture-5.html
def hz2bark_1961(Hz):
    return 13.0 * numpy.arctan(0.00076 * Hz) + 3.5 * numpy.arctan((Hz / 7500.0) ** 2)


def hz2bark_1990(Hz):
    bark_scale = (26.81 * Hz) / (1960 + Hz) - 0.5
    return bark_scale


def hz2bark_1992(Hz):
    return 6 * numpy.arcsinh(Hz / 600)


def hertz2bark_1977(hertz):
    freq_bark = 7.0 * math.asinh(hertz/650.0)  # Convert Hz to bark
    return freq_bark

#hertz = 650.0 * sinh (bark / 7.0),
def bark2hertz_1977(bark):
    hertz = 650.0 * math.sinh (bark / 7.0)
    return hertz






#erb=Alog10(1+hz*0.00437))
#where A=1000log(10)/(24.7*4.37)

# print(math.log(10)*1000/(24.7*4.37))
#21.33228113095402

def freq2erb_matlab2023b(f):
    """ Convert Hz to ERB number """
    n_erb = 21.3323 * numpy.log10(1 + 0.00437 * f)
    return n_erb

def erb2freq_matlab2023b( n_erb):
        freq_Hz = (numpy.power(10,n_erb/21.3323) - 1)/0.00437
        return freq_Hz

#computing filter banks is applying triangular filters, typically 40 filters, nfilt = 40
#sample_rate: 44100Hz or 8000Hz

def gen_mel_filter_bank(NFFT,sample_rate,nfilt):
    low_freq_mel = 0
    high_freq_mel = (2595 * numpy.log10(1 + (sample_rate / 2) / 700))  # Convert Hz to Mel
    mel_points = numpy.linspace(low_freq_mel, high_freq_mel, nfilt + 2)  # Equally spaced in Mel scale
    hz_points = (700 * (10**(mel_points / 2595) - 1))  # Convert Mel to Hz
    bin = numpy.floor((NFFT + 1) * hz_points / sample_rate)

    fbank = numpy.zeros((nfilt, int(numpy.floor(NFFT / 2 + 1))))
    cf=[]
    bw=[]
    for m in range(1, nfilt + 1):
        f_m_minus = int(bin[m - 1])   # left
        f_m = int(bin[m])             # center
        f_m_plus = int(bin[m + 1])    # right

        rnge=( hz_points[m+1] - hz_points[m-1]   )
        rnge_bin = f_m_plus - f_m_minus + 1

        cf.append(hz_points[m])
        bw.append(rnge)

        # Normalise Mel filters to constant energy of 1
        amp = 2.0 / rnge_bin

        for k in range(f_m_minus, f_m):
            fbank[m - 1, k] = (k - bin[m - 1])*amp / (bin[m] - bin[m - 1])
        for k in range(f_m, f_m_plus):
            fbank[m - 1, k] = (bin[m + 1] - k)*amp / (bin[m + 1] - bin[m])
    return fbank,cf,bw


def hertz2bark_matlab(hz):
    bark=(26.81*hz)/(1960+hz)-0.53
    if bark<2:
        bark=bark+(0.15)*(2-bark)
    if bark>20.1:
        bark=bark+(0.22)*(bark-20.1)
    return bark

def bark2hertz_matlab(bark):
    if bark<2:
        bark=(bark-0.3)/0.85
    if bark>20.1:
        bark=(bark+4.422)/1.22
    hz=1960*(bark+0.53)/(26.28-bark)
    return hz

#The Bark filter bank is designed as half-overlapped triangles equally spaced on the Bark scale	
# implementation of matlab is a reference but not fully copied
def gen_bark_filter_bank(NFFT,sample_rate,nfilt):
    low_freq_bark = hertz2bark_matlab(0)
    high_freq_bark = hertz2bark_matlab(sample_rate / 2 )  # Convert Hz to Bark
    bark_points = numpy.linspace(low_freq_bark, high_freq_bark, nfilt + 2)  # Equally spaced in Mel scale

    hz_points = numpy.zeros(len(bark_points))
    # Convert bark to Hz
    for n in range(len(bark_points)):
        hz_points [n] = bark2hertz_matlab(bark_points[n])

    bin = numpy.round((NFFT ) * hz_points / sample_rate) + 1

    fbank = numpy.zeros((nfilt, int(numpy.floor(NFFT) / 2 + 1)), dtype=float)

    linFq=[ x /NFFT * sample_rate for x in range(NFFT+1)]
    bandEdgesCastMod = [ hertz2bark_matlab(x) for x in hz_points ]
    FqMod = [hertz2bark_matlab(x) for x in linFq ]

    p = numpy.zeros(len(bandEdgesCastMod),dtype=int)
    for i in range(len(bandEdgesCastMod)):
        for index in range(len(linFq)):
            if linFq[index]> hz_points[i]:
                p[i]=index
                break


    cf=[]
    bw=[]
    for m in range(1, nfilt + 1):
        rnge=( hz_points[m+1] - hz_points[m-1]   )

        cf.append(hz_points[m])
        bw.append(rnge)

        # Normalise filters to constant energy of 1
        #  byband with
        #amp =  2.0 / rnge_bin
        # don't normalize
        amp = 1.0
        p_m_minus = p[m - 1]   # left
        p_m = p[m]             # center
        p_m_plus = p[m + 1]    # right

        bw_m=(bandEdgesCastMod[m] - bandEdgesCastMod[m - 1])
        for k in range(p_m_minus, p_m):
            fbank[m - 1 , k] = (FqMod[k] - bandEdgesCastMod[m - 1]) * amp /bw_m

        bw_m_1 = (bandEdgesCastMod[m+1] - bandEdgesCastMod[m])
        for k in range(p_m, p_m_plus):
            fbank[m - 1 , k] = (bandEdgesCastMod[m + 1] - FqMod[k]) * amp / bw_m_1

    return fbank,cf,bw

def apply_filter_bank(pow_frames,fbank):			
    filter_banks = numpy.dot(pow_frames, fbank.T)
    #filter_banks = numpy.where(filter_banks == 0, numpy.finfo(float).eps, filter_banks)  # Numerical Stability
    #filter_banks = 20 * numpy.log10(filter_banks)  # dB
    #https://haythamfayek.com/2016/04/21/speech-processing-for-machine-learning.html
    #mean normalization
    #filter_banks -= (numpy.mean(filter_banks, axis=0) + 1e-8)

    #filter_banks = numpy.where(filter_banks <numpy.finfo(float).eps, numpy.finfo(float).eps, filter_banks)  # Numerical Stability

    return filter_banks

# gammatonr filter bank. not completed yet.
def gen_erb_filter_bank( NFFT, sample_rate,  nfilt):
        low_lim=0
        # make cutoffs evenly spaced on an erb scale
        erb_low = freq2erb_matlab2023b(low_lim)
        erb_high = freq2erb_matlab2023b(sample_rate/2)
        erb_points = numpy.linspace(erb_low, erb_high, nfilt + 2)
        hz_points = erb2freq_matlab2023b(erb_points)
        bin = numpy.floor((NFFT + 1) * hz_points / sample_rate)

        cos_filts = numpy.zeros((nfilt, int(numpy.floor(NFFT / 2 + 1))))
        cf=[]
        bw=[]
        for m in range(1,nfilt+1):
            f_m_minus = int(bin[m - 1])   # left
            f_m = int(bin[m ])           # center
            f_m_plus = int(bin[m + 1])    # right

            rnge=( f_m_plus - f_m_minus  + 1 )
            hz_rnge=( hz_points[m+1] - hz_points[m-1]   )

            cf.append(hz_points[m])
            bw.append(hz_rnge)

            amp = 1.0 / rnge

            # map cutoffs to -pi/2, pi/2 interval
            for k in range(f_m_minus, f_m_plus):
                cos_filts[m-1, k] = amp * numpy.cos((k - f_m) / rnge * numpy.pi)
        return cos_filts,cf,bw






if __name__=="__main__":
#    import matplotlib.pyplot as plt
#    nfilt=40
#    sample_rate=8000
#    NFFT=256

#    plt.figure(0)


#    fbanks,cf,bw = gen_mel_filter_bank(NFFT,sample_rate,nfilt)
#    n = fbanks.shape[1]
#    step=int(numpy.floor(sample_rate/n))
#    x = numpy.arange(0,n)  # start,stop,step
#    for i in range(nfilt):
#       plt.plot(x,fbanks[i])

#    print(cf)
#    print(bw)

    nfilt=32
    sample_rate=44100
    NFFT=1024
#    plt.figure(1)


    fbanks,cf,bw = gen_bark_filter_bank(NFFT,sample_rate,nfilt)
#    n = fbanks.shape[1]
#    step=int(numpy.floor(sample_rate/n))
#    x = numpy.arange(0,n)  # start,stop,step
#    for i in range(nfilt):
#       plt.plot(x,fbanks[i])
#       print(fbanks[i])
    numpy.save("bark-fbank",fbanks)
    numpy.save("bark-fbank-cf", cf)
    numpy.save("bark-fbank-bw", bw)

#    print(cf)
#    print(bw)

#    nfilt=24
#    sample_rate=8000
#    plt.figure(2)

    # not completed yet.
#    fbanks,cf,bw = gen_erb_filter_bank(NFFT,sample_rate,nfilt)
#    n = fbanks.shape[1]
#    step=int(numpy.floor(sample_rate/n))
#    x = numpy.arange(0,n)  # start,stop,step
#    for i in range(nfilt):
#       plt.plot(x,fbanks[i])

#    print(cf)
#    print(bw)
#    plt.show()
